/*
 * Physics.h
 *
 * Created 12/1/2008 By Johnny Huynh
 *
 * Version 00.00.01 12/1/2008
 *
 * Copyright Information:
 * All content copyright © 2008 Johnny Huynh. All rights reserved.
 */
 
 #ifndef PHYSICS_H
 #define PHYSICS_H
 
 #include "Vector3.h"
 #include <math.h>
 /*
 #ifndef TYPENAME
 #define TYPENAME GLfloat
 #endif // TYPENAME
 */
 
 // Physics.h contains functions for physics calculations
 
 /** GLOBAL FUNCTIONS **/
 
 /**
  * processCollision() calculates the new velocities of objects A and B after the two 
  * collides, given the location A and B comes into contact, the (current) velocities 
  * of A and B, the masses of A and B, and the locations of objects A and B.
  *
  * @param (const Vector3<TYPENAME>)collisionPoint,
  *        (const Vector3<TYPENAME>&)pA, (Vector3<TYPENAME>&)vA, (const TYPENAME&)mA, 
  *        (const Vector3<TYPENAME>&)pB, (Vector3<TYPENAME>&)vB, (const TYPENAME&)mB
  */
 template <typename TYPENAME>
 static void processCollision( const Vector3<TYPENAME> collisionPoint,
                               const Vector3<TYPENAME>& pA, Vector3<TYPENAME>& vA, const TYPENAME& mA,  
                               const Vector3<TYPENAME>& pB, Vector3<TYPENAME>& vB, const TYPENAME& mB )
 {
    Vector3<TYPENAME> u( pA - collisionPoint );   // u = vector from the point of collision to center of A
    Vector3<TYPENAME> v( pB - collisionPoint );   // v = vector from the point of collision to center of B
    
    // convert u and v into unit vectors
    u.normalize();
    v.normalize();
    
    TYPENAME magvA = vA.getMagnitude();
    TYPENAME magvB = vB.getMagnitude();
    
    Vector3<TYPENAME> tempvA = (mA*vA) + (magvB*mB*u);
    Vector3<TYPENAME> tempvB = (mB*vB) + (magvA*mA*v);
    
    if ( tempvA.isZeroVector() & !vA.isZeroVector() )
        tempvA = -(mA*vA);
    if ( tempvB.isZeroVector() & !vB.isZeroVector() )
        tempvB = -(mB*vB);
    
    // velocities retain the magnitude of their velocities if the other 
    // object is a static object
    if ( vA.isZeroVector() )
        tempvB += magvB*mB*v;
    if ( vB.isZeroVector() )
        tempvA += magvA*mA*u;
    
    vA.normalize();
    vB.normalize();
    tempvA.normalize();
    tempvB.normalize();

    TYPENAME modA = ((dotProduct(vB, v) + 1)/2)*magvA*mA;
    TYPENAME modB = ((dotProduct(vA, u) + 1)/2)*magvB*mB;
    Vector3<TYPENAME> newvA = (tempvA/mA)*( (magvA*mA) + modB - modA );
    Vector3<TYPENAME> newvB = (tempvB/mB)*( (magvB*mB) - modB + modA );
    
    // Prevents static objects from moving. (This should be taken out for real simulation).
    //if ( !vA.isZeroVector() )
        vA = newvA;
    //if ( !vB.isZeroVector() )
        vB = newvB;
 }
 
 //#undef TYPENAME
 
 #endif // PHYSICS_H